/**
*****************************************************************************
 * @file func_StepMotor.c
 * @brief 步进电机功能函数
 * @author Moon
 * @version V1.0.0
 * @date 2022/09/04
****************************************************************************
 */
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "func_StepMotor.h"
#include "step_run_table.h"
#include "expand_io.h"
/* Private defines ---------------------------------------------------------------*/
/* Private Variable ---------------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/

//Pointer
//Array
//Const

static int StepMotor_Set_Position_unit(
	func_StepMotor_t *step_motor, //带单位运动
	uint8_t type,
	float distance,
	STEP_RUN_UNIT_e unit);
int StepMotor_Set_Position(func_StepMotor_t *step_motor, uint8_t type, float distance);
static void runover_callback(func_StepMotor_t *this);
static void StepMotor_MoveJerk(func_StepMotor_t *step_motor);
static void StepMotor_Set_reduction_ratio(func_StepMotor_t *this,
								   float reduction_ratio);
static void StepMotor_Set_Screw_Lead(func_StepMotor_t *this,
							  float lead);
static void StepMotor_Set_Speed(func_StepMotor_t *this, float speed);
static int set_step_dir(func_StepMotor_t *this, MotorTrunDif_t dir);
static STEP_MotorWorkStatusDif_t get_status(func_StepMotor_t *this);

/***********************************************************************************
 * @brief 设置绝对/相对位置  带单位进行单位转换
 * ex:
 * @par type：绝对/相对运动 (暂未使用)
        distance：绝对位置 或者 相对位置
		unit:单位
 * None
 * @retval -1 电机繁忙   
 **********************************************************************************/
static int StepMotor_Set_Position_unit(
	func_StepMotor_t *step_motor, //带单位运动
	uint8_t type,
	float distance,
	STEP_RUN_UNIT_e unit)
{
	if (step_motor == NULL)
		return -1;
	switch (unit)
	{
		case RUN_M: // convert to mm
			distance = distance * 1000.0f;
			break;
		case RUN_CM: // convert to mm
			distance = distance * 10.0f;
			break;
		case RUN_MM: 
			distance = distance;
			break;
		case RUN_UM: // convert to mm
			distance = distance / 1000.0f;
			break;
		case RUN_NM: // convert to mm
			distance = distance / 1000000.0f;
			break;
		case RUN_PULSE:
			distance = pluse_turn_distance(step_motor, distance);
			break;
		case RUN_ANGLE:
			distance = distance;
			break;
		case RUN_GRAD:
			distance = distance / 60.0f;
			break;

		default:

			return 0;
	}
	StepMotor_Set_Position(step_motor, type, distance);
	return 0;
}

/***********************************************************************************
 * @brief 设置绝对/相对位置 
 * ex:
 * @par type：绝对/相对运动
        distance：绝对位置 或者 相对位置
 * None
 * @retval -1 电机繁忙   
 **********************************************************************************/
int StepMotor_Set_Position(func_StepMotor_t *step_motor, uint8_t type, float distance)
{
	MotorAccPar_Struct_t *acc = &step_motor->private.MotorAcc;

	if (step_motor->private.step_status == STEP_MOTOR_RUN) //判断电机是否就绪
	{
		return -1;
	}
	
	if (distance == 0) //判断相对运行是否为0
	{
		return -1;
	}
	
	if (distance < 0) //根据当前位置  生成电机运动方向
	{
		if (step_motor->set_step_dir(step_motor, Motor_Reverse) < 0) 				 //设置方向
			return -1;
	}
	else
	{
		if (step_motor->set_step_dir(step_motor, Motor_Forward) < 0) 				 //设置方向
			return -1;
	}
	
	step_motor->private.position_tar = step_motor->private.position_cur + distance; //生成目标位置

	step_motor->private.rel_position_tar =
		fabsf((step_motor->private.position_tar - step_motor->private.position_cur)); //生成相对运行距离

	step_motor->private.rel_pulse_tar =
		distance_turn_pluse(step_motor, step_motor->private.rel_position_tar) + 0.5f; //四舍误入取整  误差不处理

	step_motor->private.pulse_cur = distance_turn_pluse(step_motor, step_motor->private.position_cur);
	step_motor->private.pulse_tar = distance_turn_pluse(step_motor, step_motor->private.position_tar);

	step_motor->private.rel_detec_pulse_tar = step_motor->private.rel_pulse_tar;
	if (step_motor->private.rel_pulse_tar == 0)
	{
		return -1;
	}
	else if (step_motor->private.rel_pulse_tar <= (acc->Reduce_Curve_Sum + acc->ACC_Curve_Sum))
	{
		acc->sport_type = UNIFORM_SPORT;
		Plot_T_UniformCurve(step_motor, step_motor->private.rel_pulse_tar,
							step_motor->uniform_speed);
	}
	else
	{
		acc->sport_type = S_ACCELERATE_SPORT;
		Plot_T_UniformCurve(step_motor,
							step_motor->private.rel_pulse_tar - (acc->Reduce_Curve_Sum + acc->ACC_Curve_Sum),
							step_motor->speed);
	}

	Loading_SportCurve(step_motor, acc->sport_type); //加载运动曲线值
	Scan_SportCurve(step_motor);					 //扫描运动曲线

	step_motor->private.step_status = STEP_MOTOR_RUN; //电机运行中
	step_motor->res_set_run_control(1);

	return 0;
}

/***********************************************************************************
 * @brief 无极运行，正转和反转
 * ex:
 * @par   dir：正转和反转
        
 * None
 * @retval -1 电机繁忙  -2 已触发限位开关 
 **********************************************************************************/
static int StepMotor_Run(func_StepMotor_t * step_motor,        
                         MotorTrunDif_t dir)
{
	if (step_motor->private.step_status != STEP_MOTOR_STOP) //判断电机是否就绪
	{
		step_motor->res_set_run_control(0);
		
		SoftDelayUs(1000000);
	}
	
	//if (step_motor->private.step_status != dir){ // 判断电机是否就绪
		if (step_motor->set_step_dir(step_motor, dir) < 0) 				 //设置方向
			return -1;
		

		SoftDelayUs(500000);
		
		step_motor->private.step_status = STEP_MOTOR_RUN; //电机运行中
		//step_motor->res_set_step_run(100, 0xFFFF);
		//step_motor->StepMotor_Set_Speed(step_motor,200);
		
		step_motor->res_set_run_control(1);
		
		//step_motor->StepMotor_Set_Speed(step_motor,100);
	//}
	
	return 0;
}

/***********************************************************************************
 * @brief 无极运行，正转和反转
 * ex:
 * @par   dir：正转和反转
        
 * None
 * @retval -1 电机繁忙  -2 已触发限位开关 
 **********************************************************************************/
static int StepMotor_Stop(func_StepMotor_t * step_motor)
{
	if (step_motor->private.step_status != STEP_MOTOR_STOP) //判断电机是否就绪
	{
		step_motor->res_set_run_control(0);
		
		SoftDelayUs(1000000);
	}
	
	step_motor->private.step_status = STEP_MOTOR_STOP; 
	return 0;
}

/***********************************************************************************
 * @brief 运动返回绘制曲线
 * ex:
 * @par 
 * None
 * @retval void None
 **********************************************************************************/
static void runover_callback(func_StepMotor_t *this)
{
	Scan_SportCurve(this);
}

/***********************************************************************************
 * @brief 电机急停
 * ex:
 * @par 
 * None
 * @retval void None
 **********************************************************************************/
static void StepMotor_MoveJerk(func_StepMotor_t *step_motor)
{
	if (step_motor == NULL || step_motor->res_set_run_control == NULL)
		return;
	
	step_motor->private.step_status = STEP_MOTOR_STOP;
	step_motor->res_set_run_control(0);
}

/***********************************************************************************
 * @brief 设置电机减速比
 * ex:
 * @par 
 * None
 * @retval void None
 **********************************************************************************/
static void StepMotor_Set_reduction_ratio(func_StepMotor_t *this,
								   float reduction_ratio)
{
	this->reduction_ratio = reduction_ratio;
	Plot_S_AccCurve(this, this->speed);
	Plot_S_ReduceCurve(this, this->speed);
}

/***********************************************************************************
 * @brief 设置丝杆导程
 * ex:
 * @par 
 * None
 * @retval void None
 **********************************************************************************/
static void StepMotor_Set_Screw_Lead(func_StepMotor_t *this,
							  float lead)
{
	this->unit_travel = lead;
	Plot_S_AccCurve(this, this->speed);
	Plot_S_ReduceCurve(this, this->speed);
}

/***********************************************************************************
 * @brief 设置速度
 * ex:
 * @par 
 * None
 * @retval void None
 **********************************************************************************/
static void StepMotor_Set_Speed(func_StepMotor_t *this, float speed)
{
	uint32_t freq = 0;

	if (this == NULL || this->res_set_step_run == NULL)
		return;
		
	this->speed = speed;
	freq = rpm_turn_freq(this, this->speed);

	this->res_set_step_run(freq, 0xFFFF);
	step_run_table_init(this);
}

/***********************************************************************************
 * @brief 电机转向
 * ex:
 * @par 
 * None
 * @retval 
 **********************************************************************************/
static int set_step_dir(func_StepMotor_t *this, MotorTrunDif_t dir)
{
	if (this == NULL || this->res_set_step_dir == NULL)
		return -1;
	
//	if (this->res_limit_io != NULL)
//	{
//		if (this->res_limit_io(FORWARD_LIMITE) == this->limit_polarity && dir == Motor_Forward)
//			return -1;

//		if (this->res_limit_io(REVERSE_LIMITE) == this->limit_polarity && dir == Motor_Reverse)
//			return -1;
//	}

	this->dir = dir;
	this->res_set_step_dir((uint8_t)dir);
	return 0;
}

/***********************************************************************************
 * @brief 限位触发
 * ex:
 * @par 
 * None
 * @retval 
 **********************************************************************************/
void limit_callback(func_StepMotor_t *this, LIMITE_e limit)
{
	if (this == NULL)
		return;
	
	//停止电机并改变电机状态
	this->res_set_run_control(0);    
	
	switch (limit)
	{
		case FORWARD_LIMITE:
			this->private.step_status = STEP_MOTOR_FORWARD_LIMIT;
			break;

		case REVERSE_LIMITE:
			this->private.step_status = STEP_MOTOR_REVERSE_LIMIT;
			break;	
		default:
			break;
	}
}

/***********************************************************************************
 * @brief 获取当前工作状态
 * ex:
 * @par 
 * None
 * @retval 
 **********************************************************************************/
static STEP_MotorWorkStatusDif_t get_status(func_StepMotor_t *this)
{
	return this->private.step_status;
}

/***********************************************************************************
 * @brief 步进电机管理函数初始化
 * ex:
 * @par
 * None
 * @retval void None
 **********************************************************************************/
int Func_StepMotor_Init(func_StepMotor_t *this)
{
	this->res_set_step_dir(this->dir);

	this->StepMotor_MoveJerk = StepMotor_MoveJerk;
	this->StepMotor_Set_Speed = StepMotor_Set_Speed;
	this->StepMotor_Set_reduction_ratio = StepMotor_Set_reduction_ratio;
	this->StepMotor_Set_Screw_Lead = StepMotor_Set_Screw_Lead;
	this->set_step_dir = set_step_dir;
	this->StepMotor_Run = StepMotor_Run;
	this->StepMotor_Stop = StepMotor_Stop;
	this->StepMotor_Set_Position_unit = StepMotor_Set_Position_unit;
	this->runover_callback = runover_callback;
	this->limit_callback = limit_callback;
	this->get_status = get_status;

	memset(&this->private, 0, sizeof(this->private));
	this->private.step_status = STEP_MOTOR_STOP;
	//StepMotor_Set_Speed(this, this->speed);

	this->res_set_run_control(0);
	return 0;
}
